/*
 * advance.h
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   advance.h
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   9 nov 2011
 * @brief  Extruder advance pressure management system
 *
 *
 * The advance pressure management concept was thought up by Matthew Roberts
 * and was first implemented in his own custom RepRap firmware (see 
 * http://reprap.org/wiki/Mattroberts'_Firmware). This system models the
 * extruder as a spring driving an incompressible fluid through the nozzle. The
 * pressure applied to this spring can be increased by turning the extruder
 * motor forward. This will increase the pressure inside the melting chamber,
 * which will force some of the print material out of the nozzle, until the 
 * pressure on the spring is back to normal. To extrude at higher speeds, more
 * pressure will need to be applied to the spring.
 * 
@verbatim
 The displacement to apply to the spring to achieve a certain desired flow 
 rate (mm/sec of extruded material) can be calculated using Hooke's law and
 Bernoulli's principle of incompressible fluids:

 Hooke's law: F = kx                                                 (eq. 1)
  Where F = the force exerted by the spring in N or kg*m/s²
        k = the spring constant of the specific spring used in N/m or kg/s²
        x = the displacement of the spring's end from its equilibrium
            position in m

 Bernoulli's principle: V²/2 + g.z + P/r = constant                   (eq. 2)
  Where V = desired flow rate in m/s
        g = acceleration due to gravity in m/s²
        z = height above 'ground' in m
        P = pressure in N/m² or kg/(m*s²)
        r = density in kg/m³

 We also know that the total volume of material going into the extruder must
 come out of the extruder at the bottom. This allows us to relate the ingoing
 material speed Vin to the outgoing material speed Vout:
   Vin * Ain = Vout * Aout                                            (eq. 3)
  Where Ain  = surface area of the material going in (material area)
        Aout = surface area of the material going out (nozzle area)

 Model:
          |         |  | Pa + Ps: atmospheric pressure + pressure by spring
          |     || \|/ | Vin: flow rate of material going in (mm/min)
          |     ||  '  |
          |_____||_____|
          |            |
          |            |
          |            |
          |____    ____|
               |  |
                 Pa: atmospheric pressure
                 Vout: flow rate of material going out

  Simplifications and assumptions:
   * We ignore the difference in height above ground at the top of the
     extruder and at the bottom near the nozzle.
   * We assume the density of the extruded material is constant at all points
     in the extruder.

  This leads to the following derivation:

            Vin²/2 + g.h + (Pa + Ps)/r = Vout²/2 + g.h + Pa/r
                                      /\ 
                                      ||  (by eq. 3)
                                      \/
    (Vout * Aout/Ain)²/2 + (Pa + Ps)/r = Vout²/2 + Pa/r
                                      /\ 
                                      ||  
                                      \/
        (Vout² * Aout²/Ain² - Vout²)/2 = (Pa - Pa - Ps)/r
                                      /\ 
                                      ||  
                                      \/
            (Aout²/Ain² - 1)/2 * Vout² = -Ps/r
                                      /\ 
                                      ||  
                                      \/
                                 Vout² = 2/(r * (1 - Aout²/Ain²)) * Ps

  So the square of the outgoing flow rate is proportional to the pressure
  applied on the material by the spring (which is what we would expect).

 Now we can use Hooke's law to write Ps in terms of the spring displacement:
  Ps = k * x / Ain
 
 Which leads to:
   Vout² = c * x
  With c = (2 * Ain * k)/(r * (Ain² -  Aout²)) in m/s²

 Which can be rewritten to:
   x = Vout²/c
  Where we can view x as the 'advance' distance needed to obtain the flow
  rate Vout.

 Now, all of the above calculations assumed we are given the desired _output_
 flow rate from the host. However, since Skeinforge 40, the popular Dimension
 plugin specifies the desired _input_ flow rate. Luckily, we can easily 
 convert between the two using equation 3:
   x = (Vin * Ain/Aout)² / c
  Which simplifies to:
   x = Vin² * c'
  With c' = r * Ain * (Ain² - Aout²) / (2 * k * Aout²) in s²/m

 Vin is the instant extruder feed rate, which changes during acceleration and
 deceleration. We can get the 'overall' instant output feed rate based on the
 number of timer ticks between two steps:
   f = TICKS_PER_MIN/timer_ticks_per_step * mm_per_step  in mm/min

 Normally, the instant extruder output feed rate is equal to the 'overall'
 instant feed rate, because the extruder should extrude exactly the length of
 filament that it travels above the build platform. However, the host software
 might want to pull some tricks by extruding more or less filament. To support
 this, we can calculate the instant input extruder feed rate by multiplying
 f by the block's direction vector e-component:
  Vin = f * direction[E_AXIS]
     = TICKS_PER_MIN/timer_ticks_per_step * mm_per_step * direction[E_AXIS]
   in mm/min

 Unfortunately, using the instant input extruder feed rate to calculate the
 advance steps seems to give some problems due to the limited precision of
 the input g-code commands. This can cause some strange behaviour where
 filament is extruded and retracted for no apparent reason. Therefore, it
 might be prefered to base the advance calculations on the overall instant
 input feed rate afterall, which can be calculated as follows:
  Vin' = f * (FILAMENT_OUTPUT_AREA / FILAMENT_INPUT_AREA)
       = TICKS_PER_MIN/timer_ticks_per_step * mm_per_step * 
          (FILAMENT_OUTPUT_AREA / FILAMENT_INPUT_AREA)
   in mm/min

 By setting the ADVANCE_BASE_FEEDRATE_E configuration option, the instant 
 input extruder feed rate Vin will be used. By disabling the
 ADVANCE_BASE_FEEDRATE_E configuration option, the overall instant input feed
 rate Vin' will be used instead.

@endverbatim
 */

#ifndef ADVANCE_H
#define ADVANCE_H

#if ADVANCE_ALGORITHM

#include "yarf.h"
#include "block.h"
#include "util/math.h"
#include "util/fixed_point.h"

/**
 * The maximum step frequency of the extruder motor times two.
 *
 * This is the frequency at which the 'advance interrupt service routine' is
 * called. This ISR sets or clears the extruder stepper motor's step line 
 * whenever necessary, based on the current number of advance steps that need
 * to be taken.
 */
#define EXTRUDER_HZ  (2.0*E_MAX_ADVANCE_SPEED*E_STEPS_PER_MM/60.0)

/**
 * The number of timer 1 ticks needed to obtain a frequency of \a EXTRUDER_HZ.
 */
#define EXTRUDER_TIMER_TICKS ((unsigned int)lround(F_CPU/8/EXTRUDER_HZ))

/**
 * The square of the area of the filament that is fed into the extruder.
 */
#define FILAMENT_INPUT_AREA_SQUARED (FILAMENT_INPUT_AREA*FILAMENT_INPUT_AREA)
/**
 * The square of the area of the filament that comes out of the extruder.
 */
#define FILAMENT_OUTPUT_AREA_SQUARED (FILAMENT_OUTPUT_AREA*FILAMENT_OUTPUT_AREA)
/**
 * The constant used to calculate the number of advance steps to take in order
 * to obtain a certain flow rate, according to the theory explained above.
 * The dimensions of this constant are min²/mm.
 */
#define ADVANCE_CONSTANT (FILAMENT_DENSITY * FILAMENT_INPUT_AREA *           \
        (FILAMENT_INPUT_AREA_SQUARED - FILAMENT_OUTPUT_AREA_SQUARED) /       \
        (2.0 * EXTRUDER_K * FILAMENT_OUTPUT_AREA_SQUARED))
/**
 * The constant used to calculate the number of advance steps to take in order
 * to obtain a certain flow rate, multiplied by the number of steps the
 * extruder has to take to feed in one millimeter of filament. The dimensions
 * of this constant are (min² * steps)/mm².
 *
 * This constant is to be used when the ADVANCE_BASE_FEEDRATE_E configuration
 * option is selected.
 */
#define ADVANCE_MULTIPLIER_E (ADVANCE_CONSTANT * E_STEPS_PER_MM_IN)

/**
 * The constant used to calculate the number of advance steps to take in order
 * to obtain a certain flow rate, multiplied by the number of steps the
 * extruder has to take to feed in one millimeter of filament. The dimensions
 * of this constant are (min² * steps)/mm².
 *
 * This constant is to be used when the ADVANCE_BASE_FEEDRATE_E configuration
 * option is not selected.
 */
#define ADVANCE_MULTIPLIER_O (ADVANCE_CONSTANT * E_STEPS_PER_MM_OUT)
// ADVANCE_MULTIPLIER should be between about 0.1 - 10, so will fit in a
// fxp16u16

/**
 * The extra number of steps the extruder must reverse when no filament needs
 * to be extruded, based on the \a ADVANCE_DEPRIME_MM_OUT configuration
 * parameter.
 */
#define ADVANCE_DEPRIME_STEPS lround(ADVANCE_DEPRIME_MM_OUT * E_STEPS_PER_MM_OUT)

/**
 * Returns a value which is constant throughout a block and which can be used
 * to calculate the number of advance steps later on. This precalculation saves
 * CPU time later on, when the advance steps need to be calculated.
 *
 * @param b pointer to the block for which to calculate the constant multiplier
 * @return A value which is constant throughout a block and which can be used
 *         to calculate the number of advance steps later on. 
 */
inline float
advance_get_multiplier(block_t *b) // about 5e+09
{
  // TODO: use fixed point math
#if ADVANCE_BASE_FEEDRATE_E
  return MAX(fsquare(b->mm_ticks_per_step_min) * b->direction[E_AXIS] * ADVANCE_MULTIPLIER_E, 0.0);
#else
  return MAX(fsquare(b->mm_ticks_per_step_min) * ADVANCE_MULTIPLIER_O, 0.0);
#endif
}

/**
 * Returns the number of steps to advance the extruder, in order to obtain the
 * flow rate required at the current position in the block.
 *
 * @param b pointer to the block for which to calculate the number of extruder
 *        advance steps
 * @return The number of steps to advance the extruder, in order to obtain the
 *         flow rate required at the current position in the block.
 */
inline int32_t 
advance_get_value(block_t *b)
{
  if (b->use_advance) {
    // TODO: use fixed point math
    float advance = b->advance_multiplier/fsquare(fxp16u16_to_float(b->timer_ticks));
    return (int32_t)advance;
  } else {
    return -ADVANCE_DEPRIME_STEPS;
  }
}

#endif

#endif //ADVANCE_H
